#include "includes/stereorig.h"


StereoRig::StereoRig(int _CameraLeftId, int _CameraRightId, int _ImageWidth, int _ImageHeight)
{
    SetCaptureParams(_CameraLeftId, _CameraRightId, _ImageWidth, _ImageHeight);
    LoadCameraParams();
    LoadRectifyMapParams();
    std::cout << "\n Camera Initialization!" << std::endl;
}

StereoRig::~StereoRig()
{
    this->_Extrinsic_R.release();
    this->_Extrinsic_T.release();
    this->_Extrinsic_Q.release();
    this->_Extrinsic_R1.release();
    this->_Extrinsic_R2.release();
    this->_Extrinsic_P1.release();
    this->_Extrinsic_P2.release();

    this->_Intrinsic_M1.release();
    this->_Intrinsic_M2.release();
    this->_Intrinsic_D1.release();
    this->_Intrinsic_D2.release();

    this->CaptureL.release();
    this->CaptureR.release();
    this->FrameCaptureL.release();
    this->FrameCaptureR.release();

    this->_RectifyMap_mx1.release();
    this->_RectifyMap_mx2.release();
    this->_RectifyMap_my1.release();
    this->_RectifyMap_my2.release();

    this->imgDisparity8U.release();
    this->imgDisparity16S.release();
}

void StereoRig::LoadCameraParams()
{
    std::cout << "\n Loading StereoRig Parameters." << std::endl;
    cv::FileStorage FsE;
    FsE.open("/home/tux/Tcc/Testes/build-PoseTest-Desktop-Debug/extrinsicsNewLink.yml", cv::FileStorage::READ);
    if (FsE.isOpened())
    {
        FsE["R"] >> this->_Extrinsic_R;
        FsE["T"] >> this->_Extrinsic_T;
        FsE["Q"] >> this->_Extrinsic_Q;
        FsE["R1"] >> this->_Extrinsic_R1;
        FsE["R2"] >> this->_Extrinsic_R2;
        FsE["P1"] >> this->_Extrinsic_P1;
        FsE["P2"] >> this->_Extrinsic_P2;
        FsE.release();
        FsE.~FileStorage();
    } else
    {
        std::cout << "\nProblem found on FsE Loading";
    }


    cv::FileStorage FsI;
    FsI.open("/home/tux/Tcc/Testes/build-PoseTest-Desktop-Debug/intrinsicsNewLink.yml", cv::FileStorage::READ);
    if (FsI.isOpened())
    {
        FsI["M1"] >> this->_Intrinsic_M1;
        FsI["M2"] >> this->_Intrinsic_M2;
        FsI["D1"] >> this->_Intrinsic_D1;
        FsI["D2"] >> this->_Intrinsic_D2;
        FsI.release();
        FsI.~FileStorage();
    } else
    {
        std::cout << "\nProblem found on FsI Loading";
    }
}

bool StereoRig::GetLeftImage(cv::Mat *_ImageLeft, cv::Mat *_ImageLeftGray)
{
    bool ValidFrame = true;
    std::cout << "\n Initializing Left image acquisition." << std::endl;
    cv::Mat CapturedL;
    try {
//        for (int teste = 1; teste <= 2; teste++)
        {
            this->CaptureL >> CapturedL;
        }

        if  (!CapturedL.data)
        {
            std::cout << "\n\nProblema na Leitura da câmera esquerda!";
            ValidFrame = false;
        }
    } catch (std::exception e)
    {
        std::cout << "\nAn exception occurred. Ignoring frame. " << e.what() << std::endl;
    }

    if (ValidFrame == true)
    {
        try {
            std::cout << "\n Left Image Captured." << std::endl;
            *_ImageLeft = RectifyImage(CapturedL, _RectifyMap_mx1, _RectifyMap_my1).clone();
            cvtColor(*_ImageLeft, *_ImageLeftGray, CV_BGR2GRAY);
        } catch (std::exception e) {
            std::cout << "\nAn exception occurred. Ignoring frame. " << e.what() << std::endl;
            ValidFrame = false;
        }
    }

    return ValidFrame;
}

bool StereoRig::GetRightImage(cv::Mat *_ImageRight, cv::Mat *_ImageRightGray)
{
    bool ValidFrame = true;
    std::cout << "\n Initializing Right image acquisition." << std::endl;
    cv::Mat CapturedR;
    try {
//        for (int teste = 1; teste <= 2; teste++)
        {
            this->CaptureR >> CapturedR;
        }

        if  (!CapturedR.data)
        {
            std::cout << "\n\nProblema na Leitura da câmera esquerda!";
            ValidFrame = false;
        }
    } catch (std::exception e)
    {
        std::cout << "\nAn exception occurred. Ignoring frame. " << e.what() << std::endl;
    }

    if (ValidFrame == true)
    {
        try {
            std::cout << "\n Right Image Captured." << std::endl;
            *_ImageRight = RectifyImage(CapturedR, _RectifyMap_mx2, _RectifyMap_my2).clone();
            cvtColor(*_ImageRight, *_ImageRightGray, CV_BGR2GRAY);
        } catch (std::exception e) {
            std::cout << "\nAn exception occurred. Ignoring frame. " << e.what() << std::endl;
            ValidFrame = false;
        }
    }

    return ValidFrame;
}

void StereoRig::SetCaptureParams(int _CameraLeftSysId, int _CameraRightSysId, int _CaptureWidth, int _CaptureHeight)
{
    std::cout << "\n Initializing Capture Parameters." << std::endl;
    this->CaptureL.set(CV_CAP_PROP_FRAME_WIDTH, _CaptureWidth);
    this->CaptureL.set(CV_CAP_PROP_FRAME_HEIGHT, _CaptureHeight);
    this->CaptureR.set(CV_CAP_PROP_FRAME_WIDTH, _CaptureWidth);
    this->CaptureR.set(CV_CAP_PROP_FRAME_HEIGHT, _CaptureHeight);

    this->_CapturedImageSize.height = _CaptureHeight;
    this->_CapturedImageSize.width = _CaptureWidth;

    this->FrameCaptureL.create(_CaptureWidth, _CaptureWidth, CV_8UC3);
    this->FrameCaptureR.create(_CaptureWidth, _CaptureWidth, CV_8UC3);

    this->CaptureL.open(_CameraLeftSysId);
    this->CaptureR.open(_CameraRightSysId);
}

void StereoRig::LoadRectifyMapParams()
{
    std::cout << "\n Loading Remap Parameters." << std::endl;

    cv::initUndistortRectifyMap(this->_Intrinsic_M1, this->_Intrinsic_D1, this->_Extrinsic_R1, this->_Intrinsic_M1,
                                this->_CapturedImageSize, CV_16SC2, _RectifyMap_mx1, _RectifyMap_my1);
    cv::initUndistortRectifyMap(this->_Intrinsic_M2, this->_Intrinsic_D2, this->_Extrinsic_R2, this->_Intrinsic_M2,
                                this->_CapturedImageSize, CV_16SC2, _RectifyMap_mx2, _RectifyMap_my2);
    std::cout << "\n Remap Parameters were Loaded." << std::endl;

}

cv::Mat StereoRig::RectifyImage(cv::Mat _StdImg, cv::Mat _Xmap, cv::Mat _Ymap)
{
    std::cout << "\n Image Retification." << std::endl;
    cv::Mat RectifiedImage;
    cv::remap(_StdImg, RectifiedImage, _Xmap, _Ymap, CV_INTER_CUBIC);
    return RectifiedImage;
}

cv::Mat StereoRig::GetDisparityMap(cv::Mat _ImageLeft, cv::Mat _ImageRight, cv::Mat *_Reproject3d)
{
    imgDisparity8U.create( _ImageRight.rows, _ImageRight.cols, CV_8UC1 );
    imgDisparity16S.create( _ImageRight.rows, _ImageRight.cols, CV_16S );

    int _ndisparities = 112, _SADWindowSize = 9;

    cv::StereoBM sbm( cv::StereoBM::BASIC_PRESET, _ndisparities, _SADWindowSize );
//    cv::StereoBM sbm;
//    sbm.state->SADWindowSize = 25;
//    sbm.state->numberOfDisparities = 64;
    sbm.state->preFilterSize = 5;
    sbm.state->preFilterCap = 61;
    sbm.state->minDisparity = -39;
    sbm.state->textureThreshold = 507;
    sbm.state->uniquenessRatio = 0;
    sbm.state->speckleWindowSize = 0;
    sbm.state->speckleRange = 8;



    sbm( _ImageLeft, _ImageRight, imgDisparity16S, CV_16S );

    double _minVal,_maxVal;

    cv::minMaxLoc( imgDisparity16S, &_minVal, &_maxVal);

    imgDisparity16S.convertTo( imgDisparity8U, CV_8UC1, 255/(_maxVal - _minVal));
    cv::reprojectImageTo3D(imgDisparity16S, *_Reproject3d, this->_Extrinsic_Q, true, -1);
    imgDisparity16S.release();

    return imgDisparity8U;
}
